#include "myinclude.h"
#include "Cmouse.h"


void onMouseCallbackDepth(int event, int x, int y, int flag, void* v_param) {

    if (event == CV_EVENT_LBUTTONDOWN) {
        mouseDistance * param = static_cast<mouseDistance*>(v_param);
        int y_int = (y * param->dep->height /param->resize.height);
        int x_int = (x * param->dep->width / param->resize.width);
        float* ptr_image_num = (float*) ((int8_t*) param->dep->data + y_int * param->dep->step);
        float dist = ptr_image_num[x_int];
        cv::Point3f v_dist((float)x, (float)y, dist);
        param->dist.push_back(v_dist);
        if (isValidMeasure(dist))
            printf("\n%s : %2.2f %s\n", param->name.c_str(), dist, param->unit.c_str());
        else {
            if (dist == TOO_FAR)
                printf("\n%s is too far.\n", param->name.c_str());
            else if (dist == TOO_CLOSE)
                printf("\n%s is too close.\n", param->name.c_str());
            else
                printf("\n%s not avaliable\n", param->name.c_str());
        }
    }
}



void onMouseCallbackfor3D(int event, int x, int y, int flag, void* v_param) {

    if (event == CV_EVENT_LBUTTONDOWN) {
        mouse3D * param = static_cast<mouse3D*>(v_param);
        int y_int = (y * param->dep->height /param->resize.height);
        int x_int = (x * param->dep->width / param->resize.width);


        float* ptr_image_coor = (float*) ((int8_t*) param->dep->data + y_int * param->dep->step);

        float x = ptr_image_coor[4*x_int];
        float y = ptr_image_coor[4*x_int+1];
        float z = ptr_image_coor[4*x_int+2];
        //cout<<x<<","<<y<<","<<z<<endl;
        cv::Mat wcoor(3,1,CV_32F);
        wcoor.at<float>(0,0) = ptr_image_coor[4*x_int];
        wcoor.at<float>(1,0) = ptr_image_coor[4*x_int+1];
        wcoor.at<float>(2,0) = ptr_image_coor[4*x_int+2];
        cout<<wcoor << endl;

        cv::Point3f v_dist(x, y, z);
        param->dist.push_back(v_dist);

    }
}

mouseUse::mouseUse(Mat* img, cv::Size size, UNIT zunit, std::string sname){
     /* Init mouse callback */
    // Set the structure
    resize= size;
    dep =  img;
    name = sname;
    unit = unit2str(zunit);
    cv::namedWindow(name, cv::WINDOW_AUTOSIZE);
}

mouseDistance::mouseDistance(Mat* img, cv::Size size, UNIT zunit, std::string sname) : mouseUse(img, size, zunit, sname){
    cv::setMouseCallback(name, onMouseCallbackDepth, (void *) this);
}

mouse3D::mouse3D(Mat* img, cv::Size size, UNIT zunit, std::string sname) : mouseUse(img, size, zunit, sname){

    cv::setMouseCallback(name, onMouseCallbackfor3D, (void *)this);
}


